

#include <std.h>
#include <stdio.h>
#include <string.h>
#include <log.h>
#include <tsk.h>
#include <ecm.h>
#include <gio.h>
#include "ti/pspiom/i2c/I2c.h"
#include "math.h"
#include "common.h"
#include "common_i2c.h"
#include "err.h"
#include "MS5611.h"
#include "navconbioscfg.h"





//********************************************************
//! @brief Read calibration coefficients from MS5611
//!
//! @return coefficient
//********************************************************
short MS5611_Prom_RD(GIO_Handle inhd, GIO_Handle outhd, int*status, unsigned char coef_num)
{
	short rC = 0;
	Uint8 data[2];
	data[0] = MS5611_CMD_PROM_RD + coef_num * 2;
	*status = i2cWrite(outhd,MS5611_ADDR,( unsigned char *)data,1,I2c_DEFAULT_WRITE);
	if(*status){
		return 0;
	}
	*status = i2cRead(inhd,MS5611_ADDR,( unsigned char *)data,2,I2c_DEFAULT_READ);
	if(*status){
		return 0;
	}
  	rC = (((short)data[0]) << 8) | data[1];
	return rC;
}



Uint8 MS5611_CRC4(unsigned short *n_prom)
{
    unsigned short cnt;     // simple counter
    short n_rem;    // crc reminder
	short crc_read;   // original value of the crc
	unsigned short  n_bit;
	n_rem = 0x00;
    crc_read = n_prom[7];    //save read CRC
	n_prom[7] = (0xFF00 & (n_prom[7]));  //CRC byte is replaced by 0
    for (cnt = 0; cnt < 16; cnt++)      // operation is performed on bytes
    {// choose LSB or MSB
		if (cnt%2==1)
			n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);
  		else
  			n_rem ^= (unsigned short) (n_prom[cnt>>1]>>8);
        for (n_bit = 8; n_bit > 0; n_bit--)
        {
    		if (n_rem & (0x8000))
            {
            	n_rem = (n_rem << 1) ^ 0x3000;
            }
            else
            {
            	n_rem = (n_rem << 1);
            }
        }
     }
     n_rem =  (0x000F & (n_rem >> 12));  // final 4-bit reminder is CRC code
     n_prom[7] = crc_read;   // restore the crc_read to its original place
     return (n_rem ^ 0x0);
}


int MS5611_Reset(GIO_Handle inhd, GIO_Handle outhd){
	Uint8 REG = MS5611_CMD_RESET;
	int status = 0;
	status = i2cWrite(outhd,MS5611_ADDR, &REG, 1, I2c_DEFAULT_WRITE);
	if(status){
		return status;
	}else{
		TSK_sleep(ms_conv_to_tick(MS5611_RESET_TIMING));
	}
	return status;
}


//********************************************************
//! @brief preform adc conversion for MS5611
//!
//! @return 24bit result
//********************************************************
int MS5611_CMD_ADCs(GIO_Handle inhd, GIO_Handle outhd, Uint32 * buf, Uint8 cmd)
{
	Uint8 data[8];
	int status = 0;
 	data[0] = MS5611_CMD_ADC_CONV + cmd;
 	//send conversion command
 	status = i2cWrite(outhd, MS5611_ADDR, data, 1, I2c_DEFAULT_WRITE);
 	if(status){
 		return status;
 	}
 	switch (cmd & 0x0F)     // wait necessary conversion time
	{
		case MS5611_CMD_ADC_256 :
			TSK_sleep(ms_conv_to_tick(MS5611_OSD_CONV_T_256));
			break;
	  	case MS5611_CMD_ADC_512 :
	  		TSK_sleep(ms_conv_to_tick(MS5611_OSD_CONV_T_512));
	  		break;
	  	case MS5611_CMD_ADC_1024:
	  		TSK_sleep(ms_conv_to_tick(MS5611_OSD_CONV_T_1024));
			break;
	  	case MS5611_CMD_ADC_2048:
	  		TSK_sleep(ms_conv_to_tick(MS5611_OSD_CONV_T_2048));
	  		break;
	  	case MS5611_CMD_ADC_4096:
	  		TSK_sleep(ms_conv_to_tick(MS5611_OSD_CONV_T_4096));
	  		break;
	}
	data[0] = MS5611_CMD_ADC_READ;
    //read ADC result
	status = i2cWrite(outhd, MS5611_ADDR, data, 1, I2c_DEFAULT_WRITE);
 	if(status){
 		return status;
 	}
 	status = i2cRead(inhd, MS5611_ADDR, data, 3, I2c_DEFAULT_READ);
 	if(status){
		return status;
 	}
 	*buf = (unsigned int)data[0] << 16;
 	*buf = *buf | (unsigned int)data[1] << 8;
 	*buf = *buf | data[2];

	return status;
}



int MS5611_GETDATA(GIO_Handle inhd, GIO_Handle outhd, double *data, Uint16 *coe, Uint8 mode)
{
	int status = 0;
	Uint32 D1 = 0;
	Uint32 D2 = 0;
	double BAROP_temp = 0;
	double BAROT_temp = 0;
	double BAROH_temp = 0;
	double dT = 0;
	double OFF = 0;
	double SENS = 0;
	double T2 = 0;
	double OFF2 = 0;
	double SENS2 = 0;

	status = MS5611_CMD_ADCs(inhd, outhd, &D2, MS5611_CMD_ADC_D2 + MS5611_CMD_ADC_1024);   // read D2
 	if(status){
		return status;
 	}
	status = MS5611_CMD_ADCs(inhd, outhd, &D1, MS5611_CMD_ADC_D1 + MS5611_CMD_ADC_1024);   // read D1
 	if(status){
		return status;
 	}
    // calcualte 1st order pressure and temperature
    dT = (double)(D2 - (((Uint32)coe[5]) << 8));
    OFF = ((double)coe[2]) * pow(2,16) + dT * ((double)coe[4]) / pow(2,7);
  	SENS = ((double)coe[1]) * pow(2,15) + dT * ((double)coe[3]) / pow(2,8);

  	BAROT_temp = 2000 + (dT * ((double)coe[6])) / pow(2,23);

  	if(mode == 0x02){
	  	if( BAROT_temp / 100 < 20)
	  	{
	  		T2 = (dT * dT) / pow(2,31);
	  		OFF2 = 5 * (BAROT_temp - 2000) * (BAROT_temp - 2000) / 2;
	  		SENS2 = 5 * (BAROT_temp - 2000) * (BAROT_temp - 2000) / 4;
	  		if(BAROT_temp / 100 < -15)
	  		{
	  			OFF2 = OFF2 + 7 * (BAROT_temp + 1500) * (BAROT_temp + 1500);
	  			SENS2 = SENS2 + 11 * (BAROT_temp + 1500) * (BAROT_temp + 1500) / 2;
	  		}
	  	}
	  	else
	  	{
	  		T2 = 0;
	  		OFF2 = 0;
	  		SENS2 = 0;
	  	}
	  	BAROT_temp = BAROT_temp - T2;
	  	OFF = OFF - OFF2;
	  	SENS = SENS - SENS2;
	  	//End of Two order temperature compensation
  	}
  	BAROT_temp = BAROT_temp / 100;
  	BAROP_temp = ((((double)D1 * SENS) / pow(2,21) - OFF) / pow(2,15)) / 100;
  	//BAROH_temp = pow(sea_press / BAROP_temp,1/5.257) - 1.0;
  	//BAROH_temp = BAROH_temp * (BAROT_temp + 273.15) / 0.0065;
  	BAROH_temp = ((pow((sea_press / BAROP_temp), 1/5.257) - 1.0) * (BAROT_temp + 273.15)) / 0.0065;

	data[0] = BAROP_temp;
	data[1] = BAROT_temp;
	data[2] = BAROH_temp;
	data[3] = 0;
	return status;
}


